clear
% the order of breakpoints listed below is the order used in table lookup
% data.
MS=.01;% Max sample interval
global RG
RG=250;% rounding gain
IFA=3/RG; % initial foot angle
%LUT dim. 1
APFBP=[0 IFA];% angular position of foot breakpoint vector
%LUT dim. 2
AVFBP=[0 1];
%LUT dim. 3
AAFBP=[0 1];
%LUT dim 4
APHBP=[0 1];% angular position of hip breakpoint vector
TLDT=zeros(2,2,2,2); % Table lookup data for acceleration pulse 1/2period.  Note that there are as many dimensions as breakpoint vectors, each dimension has a size = corresponding breakpoint vector length
TLDAF=zeros(2,2,2,2);
TLDST1=zeros(2,2,2,2);
%Find initial acceleration due to initia position and velocity.  LIke
%"hefting" to guage object parameters
[t,x,apf,avf,aaf,aph,apf_pe,avf_pe,aaf_pe,aph_pe,clk_pe]=sim('ipend3',.5);
%LUT dim. 3
APForder=2;% initial ap data is IFA
AVForder=1;% initial av is 0
%Run simulation once to find acceleration due to initial conditions
if aaf(1)~=0 % break point vector must be monotonic
    val=round(RG*aaf(1))/RG;
    [AAFBP AAForder]=sort([val 0]);% first data point in the acceleration break point array.
    AAForder=AAForder(1);
else
    AAFBP=[0 1];
    AAForder=1;% initial aa data is 0
end
APHorder=1;% initial aph data is 0

DATA=[];

%%
%define LUT data
%1. Arbitarily choose a pulse period, small
%2. find a pulse magnitude that will craete a swing over the vertical, and
%impart some angualr velocity opposite to direction of swing, with  little
%plateau
%3. Try to get the angular diaplacement small, but it needs to be big
%enough to allow swing-back to imapart enogh momentum
%4 need a pretty high initial magnitude movement
TLDT(APForder,AVForder,AAForder,APHorder)=25/RG;% adjust until aaf_pe(end) is about 0?
TLDAF(APForder,AVForder,AAForder,APHorder)=20000/RG;
TLDST1(APForder,AVForder,AAForder,APHorder)=0;
%execute 
tic
[t,x,apf,avf,aaf,aph,apf_pe,avf_pe,aaf_pe,aph_pe,clk_pe]=sim('ipend3',1.5);
toc
%%
[TLDT,TLDAF,TLDST1,APFBP,AVFBP,AAFBP,APHBP,APForder,AVForder,AAForder,APHorder]=InsertBlankRows(TLDT,TLDAF,TLDST1,APFBP,AVFBP,AAFBP,APHBP,apf_pe(end),avf_pe(end),aaf_pe(end),aph_pe(end));
DATA=[DATA;apf_pe(end),avf_pe(end),aaf_pe(end),aph_pe(end),clk_pe(end)];

%%
%define LUT data
%1.  Adjust ACC until APH crosses vertical slightly
%2 adjust ST1 until AVF crosses zero at pulse end and velocity is close to
%0 for a while
%3. Final AAF should be opposite in sign to ACC
ST1=0/RG;
T=25/RG;
ACC=-25793/RG;
TLDT(APForder,AVForder,AAForder,APHorder)=T;
TLDAF(APForder,AVForder,AAForder,APHorder)=ACC;
TLDST1(APForder,AVForder,AAForder,APHorder)=ST1;
%execute 
[t,x,apf,avf,aaf,aph,apf_pe,avf_pe,aaf_pe,aph_pe,clk_pe]=sim('ipend3',3);
%%
[TLDT,TLDAF,TLDST1,APFBP,AVFBP,AAFBP,APHBP,APForder,AVForder,AAForder,APHorder]=InsertBlankRows(TLDT,TLDAF,TLDST1,APFBP,AVFBP,AAFBP,APHBP,apf_pe(end),avf_pe(end),aaf_pe(end),aph_pe(end));
DATA=[DATA;apf_pe(end),avf_pe(end),aaf_pe(end),aph_pe(end),clk_pe(end)];

%%
%1. Adjust ACC so APH approaches a target.  We want APH to scillate
%slightly between 2 targets on either side of vertical
%2. try to get parameters to converge to a set of values
%3. If APH is close to zero, allwo long AT1 values, otherwise, the system
%will acquire an awkward posture
%4. Maximize the time system stays near equilibrium by using ST1 ( only for
%amsll APH values)
ST1=246/RG;
T=25/RG;
ACC=6000/RG;
TLDT(APForder,AVForder,AAForder,APHorder)=T;
TLDAF(APForder,AVForder,AAForder,APHorder)=ACC;
TLDST1(APForder,AVForder,AAForder,APHorder)=ST1;
%execute 
[t,x,apf,avf,aaf,aph,apf_pe,avf_pe,aaf_pe,aph_pe,clk_pe]=sim('ipend3',4);
%%
[TLDT,TLDAF,TLDST1,APFBP,AVFBP,AAFBP,APHBP,APForder,AVForder,AAForder,APHorder]=InsertBlankRows(TLDT,TLDAF,TLDST1,APFBP,AVFBP,AAFBP,APHBP,apf_pe(end),avf_pe(end),aaf_pe(end),aph_pe(end));
DATA=[DATA;apf_pe(end),avf_pe(end),aaf_pe(end),aph_pe(end),clk_pe(end)];

%%
%1. Adjust APH using ACC and T until it crosses zero in a monotonically
%decrasing overshoot pattern.
%2. The final AVF should be of opposite sign to ACC
%3.  want as long a eriod of 0 APF as possible
%4. Once the APH is determined, use ST1 to fine tune APF until bifurcation
%point is reached, adjust from here until proper AAF sign is achieved
ST1=155/RG;
T=25/RG;
ACC=-320/RG;
TLDT(APForder,AVForder,AAForder,APHorder)=T;
TLDAF(APForder,AVForder,AAForder,APHorder)=ACC;
TLDST1(APForder,AVForder,AAForder,APHorder)=ST1;
%execute 
[t,x,apf,avf,aaf,aph,apf_pe,avf_pe,aaf_pe,aph_pe,clk_pe]=sim('ipend3',4.5);
%%
[TLDT,TLDAF,TLDST1,APFBP,AVFBP,AAFBP,APHBP,APForder,AVForder,AAForder,APHorder]=InsertBlankRows(TLDT,TLDAF,TLDST1,APFBP,AVFBP,AAFBP,APHBP,apf_pe(end),avf_pe(end),aaf_pe(end),aph_pe(end));
DATA=[DATA;apf_pe(end),avf_pe(end),aaf_pe(end),aph_pe(end),clk_pe(end)];

%%
%1. Depending on sign of APH, make sign of ACC opposite
%2.PLANNING: for each pulse, we want the aaf to switch signs, but we also
%want APH ->0.  Set the AAF so the the next ACC will be opposite to AAF 
ST1=0/RG;
T=25/RG;
ACC=950/RG;
TLDT(APForder,AVForder,AAForder,APHorder)=T;
TLDAF(APForder,AVForder,AAForder,APHorder)=ACC;
TLDST1(APForder,AVForder,AAForder,APHorder)=ST1;
%execute 
[t,x,apf,avf,aaf,aph,apf_pe,avf_pe,aaf_pe,aph_pe,clk_pe]=sim('ipend3',8);
%%
[TLDT,TLDAF,TLDST1,APFBP,AVFBP,AAFBP,APHBP,APForder,AVForder,AAForder,APHorder]=InsertBlankRows(TLDT,TLDAF,TLDST1,APFBP,AVFBP,AAFBP,APHBP,apf_pe(end),avf_pe(end),aaf_pe(end),aph_pe(end));
DATA=[DATA;apf_pe(end),avf_pe(end),aaf_pe(end),aph_pe(end),clk_pe(end)];

%%
ST1=344/RG;
T=25/RG;
ACC=-1000/RG;
TLDT(APForder,AVForder,AAForder,APHorder)=T;
TLDAF(APForder,AVForder,AAForder,APHorder)=ACC;
TLDST1(APForder,AVForder,AAForder,APHorder)=ST1;
%execute 
[t,x,apf,avf,aaf,aph,apf_pe,avf_pe,aaf_pe,aph_pe,clk_pe]=sim('ipend3',8);
%%
[TLDT,TLDAF,TLDST1,APFBP,AVFBP,AAFBP,APHBP,APForder,AVForder,AAForder,APHorder]=InsertBlankRows(TLDT,TLDAF,TLDST1,APFBP,AVFBP,AAFBP,APHBP,apf_pe(end),avf_pe(end),aaf_pe(end),aph_pe(end));
DATA=[DATA;apf_pe(end),avf_pe(end),aaf_pe(end),aph_pe(end),clk_pe(end)];

%%
%1.  If auto match results in a pulse that generates an incorrect sign on
%AVF, then adjust amplitude of pulse and ST1 until auto generated pulse goes away
%or one is added with proper sign
ST1=220/RG;
T=25/RG;
ACC=1000/RG;
TLDT(APForder,AVForder,AAForder,APHorder)=T;
TLDAF(APForder,AVForder,AAForder,APHorder)=ACC;
TLDST1(APForder,AVForder,AAForder,APHorder)=ST1;
%execute 
[t,x,apf,avf,aaf,aph,apf_pe,avf_pe,aaf_pe,aph_pe,clk_pe]=sim('ipend3',10);
%%
[TLDT,TLDAF,TLDST1,APFBP,AVFBP,AAFBP,APHBP,APForder,AVForder,AAForder,APHorder]=InsertBlankRows(TLDT,TLDAF,TLDST1,APFBP,AVFBP,AAFBP,APHBP,apf_pe(end),avf_pe(end),aaf_pe(end),aph_pe(end));
DATA=[DATA;apf_pe(end),avf_pe(end),aaf_pe(end),aph_pe(end),clk_pe(end)];

%%
%1.  Control sign of applied ACC by getting sign of last pulse.  This may
%have been auto-generated
ST1=59/RG;
T=25/RG;
ACC=-1400/RG;
TLDT(APForder,AVForder,AAForder,APHorder)=T;
TLDAF(APForder,AVForder,AAForder,APHorder)=ACC;
TLDST1(APForder,AVForder,AAForder,APHorder)=ST1;
%execute 
[t,x,apf,avf,aaf,aph,apf_pe,avf_pe,aaf_pe,aph_pe,clk_pe]=sim('ipend3',10);
%%
[TLDT,TLDAF,TLDST1,APFBP,AVFBP,AAFBP,APHBP,APForder,AVForder,AAForder,APHorder]=InsertBlankRows(TLDT,TLDAF,TLDST1,APFBP,AVFBP,AAFBP,APHBP,apf_pe(end),avf_pe(end),aaf_pe(end),aph_pe(end));
DATA=[DATA;apf_pe(end),avf_pe(end),aaf_pe(end),aph_pe(end),clk_pe(end)];

%%
%1.  Control sign of applied ACC by getting sign of last pulse.  This may
%have been auto-generated
%2. There is a period of time where we do not want auto-completes,
%calculate each data point eplicitly, elliminating data points that cause
%auto-complete.  This forces exploration of the pose-space, and these early
%autocompletes will most likely be wrong.

ST1=152/RG;
T=25/RG;
ACC=1000/RG;
TLDT(APForder,AVForder,AAForder,APHorder)=T;
TLDAF(APForder,AVForder,AAForder,APHorder)=ACC;
TLDST1(APForder,AVForder,AAForder,APHorder)=ST1;
%execute 
[t,x,apf,avf,aaf,aph,apf_pe,avf_pe,aaf_pe,aph_pe,clk_pe]=sim('ipend3',11);
%%
[TLDT,TLDAF,TLDST1,APFBP,AVFBP,AAFBP,APHBP,APForder,AVForder,AAForder,APHorder]=InsertBlankRows(TLDT,TLDAF,TLDST1,APFBP,AVFBP,AAFBP,APHBP,apf_pe(end),avf_pe(end),aaf_pe(end),aph_pe(end));
DATA=[DATA;apf_pe(end),avf_pe(end),aaf_pe(end),aph_pe(end),clk_pe(end)];
% at this point, we've rached all zeros across the sensor values, do
% nothing until a change occurs.  To accomplish this, chneg the sample time
% from pulse end to time of first occuance of data
%%
%1.  Control sign of applied ACC by getting sign of last pulse.  This may
%have been auto-generated
%2. There is a period of time where we do not want auto-completes,
%calculate each data point eplicitly, elliminating data points that cause
%auto-complete.  This forces exploration of the pose-space, and these early
%autocompletes will most likely be wrong.
%3. Nothing says we have to go to the limit of parameters and achieve
%maximum possible results.  We should strive to accomplish the most with as
%few "good enough" values as possible.  The values determine through
%experimentation are MAXIMA, pulling back  little from these values in the
%interest of uniformity may be possible
ST1=287/RG;
T=25/RG;
ACC=-1000/RG;
TLDT(APForder,AVForder,AAForder,APHorder)=T;
TLDAF(APForder,AVForder,AAForder,APHorder)=ACC;
TLDST1(APForder,AVForder,AAForder,APHorder)=ST1;
%execute 
[t,x,apf,avf,aaf,aph,apf_pe,avf_pe,aaf_pe,aph_pe,clk_pe]=sim('ipend3',12);
%%
[TLDT,TLDAF,TLDST1,APFBP,AVFBP,AAFBP,APHBP,APForder,AVForder,AAForder,APHorder]=InsertBlankRows(TLDT,TLDAF,TLDST1,APFBP,AVFBP,AAFBP,APHBP,apf_pe(end),avf_pe(end),aaf_pe(end),aph_pe(end));
DATA=[DATA;apf_pe(end),avf_pe(end),aaf_pe(end),aph_pe(end),clk_pe(end)];

%%
manu_time=2*TLDT(APForder,1,AAForder,1);
[mindiff sampidx]=min(abs(t-.18));% find the time step closest to the end of the manuever time and sample the sensor data at this time
%add new datapoints to the break points vectors
if ~any(APFBP==apf(sampidx))% if the current sampled apf does not already exist in the apf breakpoint array
    [APFBP APForder]=sort([apf(sampidx) APFBP]);% add the new sampled value to meemory
    idx=find(APFBP==apf(sampidx));% should only be a single index returned
    APForder=APForder(idx);% use this index to modify the LUTs
end
if ~any(AVFBP==avf(sampidx))
    [AVFBP AVForder]=sort([avf(sampidx) AVFBP]);
    idx=find(AVFBP==avf(sampidx));
    AVForder=AVForder(idx);
end
apf(sampidx)
avf(sampidx)
aaf(sampidx)
aph(sampidx)
